from algo import trans, vector_dot_angle, norm, get_circle, rad_limit, get_h_line, get_cross_point
import numpy as np
from shapely.geometry import Polygon
import matplotlib.pyplot as plt
from math import sin,cos, pi

def Cal_area_2poly(data1, data2):
	"""
	任意两个图形的相交面积的计算
	:param data1: 当前物体
	:param data2: 待比较的物体
	:return: 当前物体与待比较的物体的面积交集
	"""

	poly1 = Polygon(data1).convex_hull  # Polygon：多边形对象
	poly2 = Polygon(data2).convex_hull

	if not poly1.intersects(poly2):
		inter_area = 0  # 如果两多边形不相交
	else:
		inter_area = poly1.intersection(poly2).area  # 相交面积
	return inter_area


# test
# data1 = [[0,0], [1,0], [0,1], [2,2]]
# data2 = [[0,0], [2,0], [0,2], [2,2]]
# area = Cal_area_2poly(data1, data2)
# print('area is {}'.format(area))

class CF_DWA():
	def __init__(self):
		self.theta = 270
		self.theta_d = 270
		self.t_time = 20
		self.h_time = 2
		self.r_a = 20
		self.r_b = 20

	def boxes(self, point, l, w, vb):
		# 计算bboxes
		rect_box = np.zeros((4, 3))
		# 顺时针取矩形 左下角第一个
		rect_box[0, :] = [point[0] - 0.5 * w, point[1], 0]
		rect_box[1, :] = [point[0] - 0.5 * w, point[1] + l, 0]
		rect_box[2, :] = [point[0] + 0.5 * w, point[1] + l, 0]
		rect_box[3, :] = [point[0] + 0.5 * w, point[1], 0]

		# rad 矢量和x轴的夹角 x轴为0度
		angle = vector_dot_angle(np.array([1, 0, 0]), vb)
		# 坐标系内旋转
		for pp in range(len(rect_box)):
			re = trans(point=rect_box[pp, :], angle=np.array([0, 0, angle]), mode=False)
			rect_box[pp, :] = re[0:3]

		return rect_box

	def IOU(self, box1, box2):
		# 计算IOU
		iou_value = 0
		iou_value = Cal_area_2poly(box1, box2)
		return iou_value

	# def position_update(self):
	# 	pos_new = np.array(3)
	# 	return pos_new

	# def sigmoid(self):
	# 	sig_value = 0
	#
	# 	return sig_value

	def eval(self, iou, theta):
		'''
		评估 采样速度的得分
		:param iou: 累计重叠面积
		:param theta: iou对象角度和v_perf偏差角度
		:return:
		'''
		eval_value = 0.98/(iou+1) + 0.02/(abs(theta)+1) # 0~2
		return eval_value

	def do_work(self):
		v_new = 0
		lim = 300
		lim_v = 5

		pa = np.array([-lim, -lim, 0])
		target = np.array([lim, lim, 0])

		v_opt_a = lim_v * (target-pa)/norm(target-pa)

		pb_all = np.random.rand(50, 3)
		pb_all = pb_all * lim * 2 - 0.8 * lim
		pb_all[:, 2] = 0

		v_opt_b_all = np.random.rand(50, 3)
		v_opt_b_all = (v_opt_b_all * 2 - 1) * lim_v
		v_opt_b_all[:, 2] = 0

		tt = 0
		hh = 1

		att = 1000

		plt.ion()  # 开启绘图

		while tt < att:
			# 更新v_perf
			v_perf_angle = vector_dot_angle(np.array([1, 0, 0]), target-pa)

			# l_angle 重新初始化 rad
			l_angle = (pi/180)*np.linspace(-0.5 * self.theta, 0.5 * self.theta, self.theta_d)
			# print('l_angle init is {}'.format(l_angle))
			# 求当前船的方向角 rad
			v_angle_a = vector_dot_angle(np.array([1, 0, 0]), v_opt_a)
			# 在船两侧对称l_angle采样
			l_angle = v_angle_a + l_angle
			# 约束角度在-pi~pi
			for kk in range(len(l_angle)):
				l_angle[kk] = rad_limit(l_angle[kk])

			after_angle = []
			for i in range(len(l_angle)):
				t_count = 0

				# 得到采样 v_a_simple 对应方向矩形框
				# 求采样方向矢量
				toward_simple = np.array([cos(l_angle[i]), sin(l_angle[i]), 0])
				v_toward_simple = trans(toward_simple,np.array([0,0,pi/2]),mode=False)
				# 按方向求直线方程
				pa_coe_h = get_h_line(pa, toward_simple)
				pa_coe_v = get_h_line(pa, v_toward_simple)
				# 遍历判断是否和障碍物有交点 如果t时间内存在交点,且交点在障碍物和机器人前方 flag = False break
				# 避免和障碍物平行运动 没有交点 机器人还有一条横线，与速度方向垂直，交点距离小于r_a则 false
				flag = True
				for j in range(len(pb_all)):
					pb_coe = get_h_line(pb_all[j], v_opt_b_all[j])
					point, l_flag = get_cross_point(pb_coe, pa_coe_h) # 纵向交点
					point_v, v_flag = get_cross_point(pb_coe, pa_coe_v) # 横向交点
					if l_flag:
						# 如果有交点, 判断到机器人的距离 纵向小于v*t, 横向小于r_a, 夹角小于正负90
						if (norm(point - pa) < (self.t_time*lim_v)) and \
								(90*pi/180<abs(vector_dot_angle(toward_simple, point - pa))) and\
								(norm(point - pb_all[j]) < 1.5*(self.t_time * norm(v_opt_b_all[j]))):

							flag = False
							break
					# if v_flag:
					# 	# 如果横向有交点 横向距离小于r_a
					# 	if (norm(point_v - pa) < self.r_a):
					# 		flag = False
					# 		break

				if flag:
					after_angle.append(l_angle[i])
				# 不存在交点flag = True, 记录这个采样方向

            # 直接做直线求交点采样 时间t内有交点就放弃这个采样方向
			print('after_angle is {}'.format(after_angle))

			min_angle = self.theta*pi/180

			for di in range(len(after_angle)):
				d_angle = rad_limit(after_angle[di] - v_perf_angle)
				if abs(d_angle) < min_angle:
					min_angle = abs(d_angle)
					v_new_a = after_angle[di]

			print('target angle is {}'.format(v_perf_angle))
			print('v_new_a is {}'.format(v_new_a))
			torward_new = np.array([cos(v_new_a), sin(v_new_a), 0])
			# v_opt_a = (min_t/self.t_time) * lim_v*torward_new
			v_opt_a = lim_v * torward_new
			print('v_opt_a is {}'.format(v_opt_a))

			if norm(pa - target) <= 30:
				break

			tt += hh

			# 更新pa  pb  的位置
			old_pa = pa
			v_arraw = old_pa + 20*hh*v_opt_a
			p_arraw = old_pa + self.t_time*lim_v * v_opt_a/norm(v_opt_a)
			print('arraw is {}'.format(v_arraw))
			pa = pa + hh*v_opt_a
			old_pb_all = pb_all
			pb_all = pb_all + v_opt_b_all*hh

			plt.clf()  # 清除上个绘图
			plt.plot(target[0], target[1], 'o')
			plt.plot(old_pa[0], old_pa[1], 'x')
			plt.plot([old_pa[0], v_arraw[0] ], [old_pa[1], v_arraw[1]], 'y')
			plt.plot([old_pa[0], p_arraw[0]], [old_pa[1], p_arraw[1]], 'r')
			plt.plot()
			x_a, y_a = get_circle([pa[0], pa[1]], self.r_a)
			plt.plot(x_a, y_a)
			for pb in old_pb_all:
				x_b, y_b = get_circle([pb[0], pb[1]], self.r_b)
				plt.plot(x_b, y_b)
			plt.xlim(-1.2 * lim, 1.2 * lim)
			plt.ylim(-1.2 * lim, 1.2 * lim)

			plt.pause(0.1)  # 暂停一秒
		plt.ioff()  # 关闭画图的窗口
		return v_new


cf = CF_DWA()

v_new = cf.do_work()